perm filename BEJCZY.PAL[AL,HE]3 blob
sn#339089 filedate 1978-03-07 generic text, type C, neo UTF8
COMMENT ⊗ VALID 00011 PAGES
C REC PAGE DESCRIPTION
C00001 00001
C00003 00002 BEJCZY - BEJCZY SIMPLIFIED DETERMINATION OF D MATRIX
C00005 00003 LINK INERTIAL VALUES FOR THE BLUE ARM
C00007 00004 PARAMETERS IN SIMPLIFIED STATE EQUATIONS FOR INERTIA AND GRAVITY LOADS.
C00010 00005 STORAGE AREA FOR BEJCZY COEFFICIENTS
C00012 00006 HNDWT & NOWT: MODIFY BEJCZY PARAMETERS FOR A GIVEN LOAD IN THE HAND.
C00015 00007 "NOWT/HNDWT" EXECUTABLE CODE
C00018 00008 DTERMS: COMPUTES THE D(II) AND D(I) COEFFICIENTS OF THE STATE EQUATIONS.
C00021 00009 "DTERMS" EXECUTABLE CODE
C00024 00010 [CONTINUATION OF DTERMS]
C00026 00011 [CONTINUATION OF DTERMS]
C00028 ENDMK
C⊗;
;BEJCZY - BEJCZY SIMPLIFIED DETERMINATION OF D MATRIX
.TITLE BEJCZY
;THIS FILE CONTAINS THREE ROUTINES: HNDWT,NOWT,AND DTERMS. THEY
;CAN BE USED TO DETERMINE THE DIAGONAL TERMS OF THE ACCELERATION
;COEFFICIENTS MATRIX AS WELL AS THE GRAVITY MATRIX FOR THE STATE
;EQUATIONS DEFINING THE ARM DYNAMICS. THE IMPLEMENTED EQUATIONS
;WERE DERIVED IN "ROBOT ARM DYNAMICS AND CONTROL, JPL MEMORANDUM
;33-669", BY A.K. BEJCZY.
;
;THE PURPOSE OF THE THREE ROUTINES AND THEIR REQUIRED ARGUMENTS
;ARE EXPLAINED IN THE FOLLOWING PAGES.
;DEFINITIONS
YELARM== 1 ;MECHANISM BITS: YELLOW ARM, NOT INCLUDING HAND
YELHND== 2 ;YELLOW HAND
BLUARM== 4 ;BLUE ARM, NOT INCLUDING HAND
BLUHND== 10 ;BLUE HAND
VISE==20 ;VISE
SCREW==40 ;SCREWDRIVER
;LINK INERTIAL VALUES FOR THE BLUE ARM
;
; LINK MASS X Y Z I(XX) I(YY) I(ZZ)
; [OZ-MSEC↑2/IN] [INCHES] [OZ-IN-MSEC↑2]
;
; 1 513610.0 0 0 -1.69 5.494@6
; 2 259780.0 0 -4.28 -1.67 9.6329@6 2.0459@6 7.7522@6
; 3 258787.0 0 0 -18.5 7.9694@7 7.9694@7 39167.0
; 4 101390.0 0 0 1.323 633330.0 174440.0 505560.0
; 5 41460.0 0 0 2.82 440346.0 414080.0 38837.0
; 6 33686.0 0 0 -3.09 422775.0 395629.0 39335.0
;
;INPUT SHAFT INERTIAS IN [OZ-IN-MILLISECONDS**2]:
;
; LINK INERTIA
;
; 1 1.5000@8
; 2 4.0000@8
; 3 100000.0 (MODIFICATION OF ORIGINAL BEJCZY EQUATIONS)
; 4 1.0212@7
; 5 1.0212@7
; 6 848093.0
; 7 1.5900@6
;
;THE FORCE OF GRAVITY IS TAKEN TO BE:
;
; G = 3.8592*10@-4 OZ-MILLISECONDS**2/INCHES
;
;GEOMETRICAL PARAMETERS:
;
; R2=6.05
; R6=9.38 (YELLOW), R6=10.281 (BLUE)
;PARAMETERS IN SIMPLIFIED STATE EQUATIONS FOR INERTIA AND GRAVITY LOADS.
;ALL VALUES ARE FOR THE CASE OF NO LOAD BEING HELD IN THE HAND.
;BB1: B1 = INPUT INERTIA 1 + I(1YY) + 2M(2)Y(2)R(2)
; + (M(2)+M(3)+M(4)+M(5))R(2)**2 + .5( I(2XX) + I(2ZZ) )
;BB2: B2 = I(3YY)
;BB3: B3 = 2( M(3)Z(3) - M(4)Y(4) )
;BB4: B4 = INPUT INERTIA 3 + M(3) + M(4) + M(5) + M(6)
;BB5: B5 = 2M(5)Z(5) + 2M(6)*( Z(6) + R(6) )
;BB13: B13 = INPUT INERTIA 2 + I(2YY) + I(3XX)
;BB14: B14 = INPUT INERTIA 4 + I(4YY)
;BB15: B15 = I(5XX) + I(6XX) + M(6)R(6)*( R(6) + 2Z(6) )
;BB1718: B17 + B18 = INPUT INERTIA 5 + I(5YY) + I(6XX)
; + M(6)R(6)* ( 2Z(6) + R(6) )
;BB1920: B19 + B20 = INPUT INERTIA 6 + I(6ZZ)
;BGD1: G*D1= G*( M(2)Z(2) + M(3)Z(3) - M(4)Y(4) )
;GG: G = ACCELERATION DUE TO GRAVITY
;NGG: -G
;BGD2: G*D2= G*( BB4 - INPUT INERTIA 3 )
;BMGD3: -G*D3=-G*( .5BB5 )
;BR6Z6: 2*( R(6) + Z(6) )
;BR62Z6: R(6)*( R(6) + 2Z(6) )
;BHDI: HAND GRAVITY LOADING
;BHDII: HAND INERTIA
FP BB1, 47046,152766 ; .1749400@9
FP BB2, 46630, 426 ; .7969400@8
FP BB3, 146022, 15274 ;-9575100.
FP BB4, 45002,130600 ; 535320.0
FP BB5, 45057, 57140 ; 718310.0
FP BB13, 47345,133047 ; .4817400@9
FP BB14, 46036, 75120 ; .1038600@8
FP BB15, 45413, 57040 ; 2283400.
FP BB1718, 46076, 41410 ; .1246900@8
FP BB1920, 45130,124140 ; 887430.0
FP BGD1, 143022,105463 ;-2344.700
FP GG, 35312, 52516 ; .3859200@-3
FP NGG, 135312, 52516 ;-.3859200@-3
FP BGD2, 42074,131463 ; 188.7000
;FP BGD2, 42050, 0 ; 168.0000
FP BMGD3, 142012,114632 ;-138.6000
FP BR6Z6, 41146, 16254 ; 14.38200
FP BR62Z6, 41450,122743 ; 42.16200
FP BHDI, 0,0 ; 0.0
FP BHDII, 45302, 13600 ; 1590000.
;STORAGE AREA FOR BEJCZY COEFFICIENTS
G: GG ;POSITIVE AND NEGATIVE GRAVITY CONSTANTS
NEGG: NGG
YBC: BB15 ;LIST OF BEJCZY COEFFICENTS FOR YELLOW ARM
BB14 ;ORDERED LIST- DO NOT MODIFY!!!!!!
BB1718
BB1920
BB4
BB5
BB3
BB13
BB2
BB1
BGD2
BMGD3
BGD1
BB4 ;VALUES FOR LOAD DEPENDENT COEF. WITH NO LOAD
BB5 ; IN HAND
BB15
BB1718
BB1920
BR6Z6
BR62Z6
BBC: BB15 ;LIST OF BEJCZY COEFFICENTS FOR BLUE ARM
BB14 ;ORDERED LIST - DO NOT MODIFY!!!!!!
BB1718
BB1920
BB4
BB5
BB3
BB13
BB2
BB1
BGD2
BMGD3
BGD1
BB4 ;VALUES FOR LOAD DEPENDENT COEF. WITH NO LOAD
BB5 ; IN HAND
BB15
BB1718
BB1920
BR6Z6
BR62Z6
BHDATA: BHDI ;BLUE HAND DYNAMIC COEFFICIENTS
BHDII
YHDATA: BHDI ;YELLOW HAND DYNAMIC COEFFICIENTS
BHDII
VDATA: BHDI ;VISE DYNAMIC COEFFICIENTS *K
BHDII
;HNDWT & NOWT: MODIFY BEJCZY PARAMETERS FOR A GIVEN LOAD IN THE HAND.
;HNDWT IS USED TO ESTABLISH A LOAD BEING HELD IN THE HAND AND NOWT
;RESETS THE COEFFICIENTS FOR NO LOAD. A SAMPLE CALLING SEQUENCE
;FOLLOWS:
;
; ** MOV #LOAD,R0 ;POINTER TO ARRAY WHERE LOAD DATA STORED
; MOV #MECH,R2 ;MECHANISM BIT, INDICATING WHICH ARM
; JSR PC,HNDWT ;CALLED USING PC
; or
; JSR PC,NOWT
;
;WHERE LOAD(1)←WEIGHT OF MASS IN OZ.
; LOAD(2)←RADIUS OF GYRATION ABOUT X IN INCHES
; LOAD(3)←RADIUS OF GYRATION ABOUT Y IN INCHES
; LOAD(4)←RADIUS OF GYRATION ABOUT Z IN INCHES
;
;** THIS INSTRUCTION NOT NEED FOR A CALL TO NOWT
;
;IT IS ASSUMED THAT THE CENTER OF MASS OF THE LOAD IS LOCATED AT THE ORIGIN
;OF THE HAND SYSTEM. ALL NUMBERS SHOULD BE IN SINGLE PRECISION FLOATING POINT.
;RUN TIME: 260 MICRO SEC. FOR HNDWT, 200 MICRO SEC. FOR NOWT
;REGISTERS USED:
;
; R0,R2 PASS ARGUMENTS BUT ARE NOT MODIFIED
; R1,AC0,AC1,AC2,AC3 ARE GARBAGED
;DEFINTIONS
MASS==AC0 ;LOAD MASS
IX==AC1 ;VALUE OF I(XX)
IZ==AC2 ;VALUE OF I(ZZ)
AC==AC3 ;TEMPORARY STORAGE
B15==0 ;ADDR. OF BEJCZY COEF. RELATIVE TO THE START OF THE COEF LIST
B1718==10
B1920==14
B4==20
B5==24
GD2==50
MGD3==54
N4==64 ;NO LOAD VALUE OF BEJCZY COEF.
N5==70
N15==74
N1718==100
N1920==104
R6Z6==110
R62Z6==114
; "NOWT/HNDWT" EXECUTABLE CODE
NOWT: CLRF MASS ;SET MASS,IX,IZ TO ZERO
CLRF IX
CLRF IZ
BR SAMEWT ;BRANCH TO COMMON CODE
HNDWT: LDF (R0),MASS ;GET THE MASS IN OZ.
DIVF G,MASS ;CONVERT FROM FORCE TO MASS
LDF 4(R0),IX ;GET THE VALUE OF R(XX)
MULF IX,IX ;SQUARE THE VALUE
MULF MASS,IX ;NOW HAVE I(XX)=MASS*R(XX)**2
LDF 14(R0),IZ ;GET THE VALUE OF R(ZZ)
MULF IZ,IZ ;SQUARE IT
MULF MASS,IZ ;NOW HAVE I(ZZ)=MASS*R(ZZ)**2
;COMMON CODE FOR NOWT AND HNDWT
SAMEWT: MOV #BBC,R1 ;SET POINTER TO BEJ. COEF. FOR BLUE ARM
BIT #BLUARM,R2 ;CHECK IF YELLOW OR BLUE ARM
BNE .+6 ;SKIP IF BLUE ARM
MOV #YBC,R1 ;ELSE LOAD POINTER TO YELLOW ARM BEJ COEF.
LDF N4(R1),AC ;LOAD B4
ADDF MASS,AC ;ADD THE MASS OF THE LOAD
STF AC,B4(R1) ;SAVE THE NEW VALUE OF B4
MULF G,AC ;MULT. BY ACCEL. OF GRAVITY
STF AC,GD2(R1) ;STORE IN COEF. ARRAY
LDF R6Z6(R1),AC ;LOAD 2*( R(6) + Z(6) )
MULF MASS,AC ;MULT. BY THE MASS
ADDF N5(R1),AC ;ADD NO LOAD VALUE OF B5
STF AC,B5(R1) ;STORE IN COEF. ARRAY
MULF #40000,AC ;CALCULATE D3=.5*B5
MULF NEGG,AC ;CALCULATE -G*D3
STF AC,MGD3(R1) ;STORE IN COEF. ARRAY
MULF R62Z6(R1),MASS ;MULT. MASS BY R(6)*( R(6) + 2Z(6) )
ADDF IX,MASS ;ADD THE I(XX) OF THE LOAD
STF MASS,IX ;SAVE RESULT IN IX ALSO
ADDF N15(R1),MASS ;ADD THE NO LOAD VALUE OF B15
STF MASS,B15(R1) ;STORE IN COEF. ARRAY
ADDF N1718(R1),IX ;ADD TO IX NO LOAD B17+B18
STF IX,B1718(R1) ;STORE NEW VALUE OF B17 + B18
ADDF N1920(R1),IZ ;ADD MASS I(ZZ) TO NO LOAD B19+B20
STF IZ,B1920(R1) ;STORE NEW VALUE OF B19 + B20
RTS PC ;RETURN
;END OF "NOWT/HNDWT"
;DTERMS: COMPUTES THE D(II) AND D(I) COEFFICIENTS OF THE STATE EQUATIONS.
;A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #THETA,R0 ;PTR TO LIST OF PTRS TO JOINT ANGLES
; MOV #DARRAY,R1 ;PTR TO LIST OF PTRS TO D(I)'S AND D(II)'S
; MOV #MECH,R2 ;MECHANISM BIT INDICATING DEVICE TO USE
; JSR PC,DTERMS ;CALLED USING THE PC
;
;THE D ARRAY MUST BE A LIST OF POINTERS TO THE GRAVITY LOADING AND INERTIA
;VARIABLES FOR EACH JOINT. THE LIST MUST BE ORDERED FROM D(1),...,D(6),
;D(11),...,D(66), WHERE THE D(I)'S ARE THE GRAVITY LOADING AND THE D(II)'S
;THE JOINT INERTIA TERMS. OF THESE TWELVE VARIABLES, ONLY D(2),D(3),D(4),
;D(5),D(11),D(22), AND D(44) ARE DEPENDENT ON THE THETA'S. JOINTS 1 AND 6
;ARE SUBJECT TO NO GRAVITY LOADING HENSE D(1) AND D(6) ARE ALWAYS ZERO.
;IF THE MECHANISM SPECIFIED IS A HAND, ONLY TWO FLOATING POINT NUMBERS
;ARE SENT BACK, THE HAND GRAVITY LOADING AND INERTIA.
;RUN TIME ~ 1 MILLISECOND.
;REGISTERS USED:
;
; R0,R1,R2 PASS ARGUMENTS AND ARE LEFT UNCHANGED
; AC0,AC1,AC2,AC3,AC4,AC5 ARE GARBAGED
;DEFINITIONS
SN==AC0 ;SINE RESULTS OF SNCOS RETURNED IN REG 0
CS==AC1 ;COSINE " " " " " " 1
ZAC==AC2 ;TEMPORARY STORAGE
ZBC==AC3
ZCC==AC4
ZDC==AC5
TH2==2 ;JOINT ANGLES RELATIVE TO THE ADDRESS IN R0
S3==4
TH4==6
TH5==10
KD2==2 ;GRAVITY LOADING TERMS RELATIVE TO ADDRESS IN R1
KD3==4
KD4==6
KD5==10
KD6==12
KD11==14 ;INERTIAL TERMS RELATIVE TO ADDRESS IN R1
KD22==16
KD33==20
KD44==22
KD55==24
KD66==26
;LOCAL STORAGE OF VARIABLES
SSN2: .WORD 0,0 ;SIN THETA 2
CCS2: .WORD 0,0 ;COS THETA 2
SSN5: .WORD 0,0 ;SIN THETA 5
CCS5: .WORD 0,0 ;COS THETA 5
; "DTERMS" EXECUTABLE CODE
DTERMS: MOV R2,-(SP) ;SAVE REGISTER
BIT #BLUHND+YELHND+VISE,R2 ;CHECK IF COEF. FOR HANDS|VISE REQUESTED
BNE HNDCOE ;BRANCH IF HAND COEF. NEEDED
;COMPUTE D44
LDF @TH5(R0),AC0 ;CALCULATE THE SIN/COS OF THETA 5
JSR PC,SNCOS
STF SN,SSN5 ;SAVE THE SINE OF THETA 5
STF CS,CCS5 ;SAVE COS 5 IN AC1
MULF SN,SN ; (SIN TH5)**2
MOV #BBC,R2 ;SET POINTER TO BEJ. COEF. OF BLUE ARM
BIT #BLUARM,(SP) ;CHECK IF YELLOW OR BLUE
BNE .+6 ;SKIP IF BLUE ARM
MOV #YBC,R2 ;ELSE SUBSTITUE YELLOW BEJ. COEF.
MULF (R2)+,SN ; X B15
ADDF (R2)+,SN ; + B14
STF SN,@KD44(R1) ;SAVE D44
;D33, D55, AND D66 ARE CONSTANTS DEPENDENT ONLY UPON HAND LOAD
LDF (R2)+,ZAC ;D55 = B17 + B18
STF ZAC,@KD55(R1)
LDF (R2)+,ZAC ;D66 = B19 + B20
STF ZAC,@KD66(R1)
LDF (R2)+,ZAC ;D33 = B4
STF ZAC,@KD33(R1)
;COMPUTE D22
MULF @S3(R0),ZAC ;B4 x S3
MULF (R2)+,CS ;CCS5 X B5
ADDF (R2)+,CS ; + B3
ADDF ZAC,CS ;S3*B4 + ( B3 + B5*CCS5 )
MULF @S3(R0),CS ; X S3
STF CS,ZCC ;SAVE RESULT IN ZCC TEMPORARILY
ADDF (R2)+,CS ; + B13
STF CS,@KD22(R1) ;SAVE OF D22
;COMPUTE D11
LDF @TH2(R0),AC0 ;GET VALUE OF SIN AND COS TH2
JSR PC,SNCOS
STF SN,SSN2 ;SAVE THE VALUE
MULF SN,SN ; (SIN TH2)**2
LDF ZCC,ZBC ;MOVE ( B3 + B5*CCS5)*S3 + B4*S3**2
ADDF (R2)+,ZBC ;ADD B2 TO ( B3 + B5*CCS5 )*S3 + B4*S3**2
MULF ZBC,SN ; X (SIN TH2)**2
ADDF (R2)+,SN ; + B1
STF SN,@KD11(R1) ;SAVE D11
; [CONTINUATION OF DTERMS]
;COMPUTE CD3
STF CS,CCS2 ;SAVE THE VALUE OF COS TH2
MULF (R2)+,CS ;X (G * D2)
STF CS,@KD3(R1) ;SAVE CD3
;COMPUTE CD5
LDF @TH4(R0),AC0 ;GET VALUE OF SIN AND COS TH4
JSR PC,SNCOS
LDF SSN2,ZAC ;LOAD THE SIN TH2
MULF CCS5,ZAC ; X COS TH5
STF ZAC,ZCC ;SAVE RESULT
MULF SN,ZAC ; X SIN TH4
LDF CCS2,ZBC ;GET COS TH2
MULF SSN5,ZBC ;MULT. COS TH2 BY SIN TH5
ADDF ZBC,ZAC ;NOW HAVE SIN TH2*COS TH5*SIN TH4
; + COS TH2*SIN TH5
MULF (R2),ZAC ;X (-G * D3)
STF ZAC,@KD5(R1) ;SAVE CD5
;COMPUTE CD2
MULF ZBC,SN ;NOW HAVE SIN TH4*COS TH2*SIN TH5
ADDF ZCC,SN ; + SIN TH2*COS TH5
MULF (R2)+,SN ; X (-G * D3)
LDF @S3(R0),ZAC ;G*D2*S3
MULF -10(R2),ZAC
ADDF (R2),ZAC ; + G*D1
MULF SSN2,ZAC ; X SIN TH2
SUBF ZAC,SN ; - G*D3*(SIN TH2*COS TH5
; +SIN TH4*COS TH2*SIN TH5)
STF SN,@KD2(R1) ;SAVE CD2
;COMPUTE CD4
MULF SSN5,CS ; COMPUTE COS TH4 X SIN TH5
MULF SSN2,CS ; X SIN TH2
MULF -(R2),CS ; X (-G * D3)
STF CS,@KD4(R1) ;SAVE CD4
BR DETDNE ;EXIT CLEANLY
; [CONTINUATION OF DTERMS]
;SEND BACK COEFFICIENTS FOR THE HANDS AND VISE
HNDCOE: MOV #BHDATA,R2 ;ASSUME THE BLUE HAND DATA IS REQUESTED
BIT #YELHND,(SP) ;CHECK IF YELLOW HAND INSTEAD
BEQ 1$ ;SKIP IF NOT YELLOW
MOV #YHDATA,R2 ;ELSE GET POINTER TO YELLOW HAND DATA
1$: BIT #VISE,(SP) ;CHECK IF VISE REQUESTED
BEQ 2$ ;SKIP IF NOT VISE
MOV #VDATA,R2 ;GET POINTER TO VISE DATA
2$: LDF (R2),AC0 ;TRANSFER THE GRAVITY LOADING TO SERVO BLOCK
STF AC0,@(R1)
LDF 4(R2),AC0 ;TRANSFER THE JOINT INERTIA TO SERVO BLOCK
STF AC0,@2(R1)
;RESTORE THE REGISTERS AND EXIT
DETDNE: MOV (SP)+,R2 ;RESTORE REGISTER
RTS PC ;RETURN
;END OF "BEJCZY.PAL"